/*=========================================================================
	UberSim Source Code Release
	-------------------------------------------------------------------------
	Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
	James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
	Erick Tryzelaar {erickt}@andrew.cmu.edu
	School of Computer Science, Carnegie Mellon University
	-------------------------------------------------------------------------
	This software is distributed under the GNU General Public License,
	version 2.  If you do not have a copy of this licence, visit
	www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
	Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
	in the hope that it will be useful, but WITHOUT ANY WARRANTY,
	including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	-------------------------------------------------------------------------*/

#include <stdio.h>
#include <ode/ode.h>

#include "mmgr.h"

#include "Collision.h"
#include "Island.h"
#include "Node.h"
#include "RigidBody.h"
#include "RigidBodyCollider.h"
#include "Simulator.h"
#include "Spatial.h"
#include "Surface.h"

/*******************************/

RigidBody::RigidBody ()
{
	island    = 0;
	rollingMu = 0.0;

	body = dBodyCreate (Simulator::getInstance ()->getWorld ());
}

/*******************************/

RigidBody::~RigidBody ()
{
	if (island)
	{
		if (island->getRigidBodyCount () <= 1)
		{
			delete island;
		}
		else
		{
			island->removeRigidBody (this);
		}
	}

	dBodyDestroy (body);
}

/*******************************/

Island* RigidBody::getIsland ()
{
	return island;
}

/*******************************/

dBodyID& RigidBody::getBody ()
{
	return body;
}

/*******************************/

RigidBodyCollider* RigidBody::getRigidBodyCollider ()
{
	return rigidBodyCollider;
}

/*******************************/

Vector3 RigidBody::getAppliedForce ()
{
	return dBodyGetForce (body);
}

/*******************************/

Vector3 RigidBody::getAppliedTorque ()
{
	return dBodyGetTorque (body);
}

/*******************************/

void RigidBody::setIsland (Island* island)
{
	this->island = island;
}

/*******************************/

void RigidBody::setBody (dBodyID& body)
{
	this->body = body;
}

/*******************************/

void RigidBody::setRigidBodyCollider (RigidBodyCollider* rigidBodyCollider)
{
	this->rigidBodyCollider = rigidBodyCollider;
}

/*******************************/

void RigidBody::setRollingMu (const Real rollingMu)
{
	this->rollingMu = rollingMu;
}

/*******************************/

void RigidBody::setLocalPosition (const Vector3& localPosition)
{
	// assert body

	if (island)
	{
		Vector3 deltaPosition = localPosition - getLocalPosition ();

		for (unsigned int i = 0; i < island->size (); i++)
		{
			RigidBody* rigidBody = island->getRigidBody (i);

			if (rigidBody)
			{
				dBodyID body = rigidBody->body;

				Spatial* model = rigidBody->model;
				Spatial* parent = model->getParent ();

				Vector3 position = Vector3 (dBodyGetPosition (body)) + deltaPosition;

				dBodySetPosition (body, position.x, position.y, position.z);
				model->setWorldPosition (position);

				if (parent)
				{
					model->setLocalPosition (parent->getWorldRotation ().transpose () * 
							((model->getWorldPosition () - parent->getWorldPosition ()) / parent->getWorldScale ()));
				}
				else
				{
					model->setLocalPosition (model->getWorldPosition ());
				}
			}
		}
	}
	else
	{
		model->setLocalPosition (localPosition);

		Spatial* parent = model->getParent ();

		if (parent)
		{
			model->setWorldPosition (parent->getWorldPosition () + 
					parent->getWorldScale () * (parent->getWorldRotation () * localPosition));
		}
		else
		{
			model->setWorldPosition (model->getLocalPosition ());
		}

		Vector3 worldPosition = getWorldPosition ();

		dBodySetPosition (body, worldPosition.x, worldPosition.y, worldPosition.z);
	}
}

/*******************************/

void RigidBody::setLocalRotation (const Matrix33& localRotation)
{
	// assert body

	Spatial* parent = model->getParent ();

	if (parent)
	{
		model->setWorldRotation (parent->getWorldRotation () * localRotation);
	}
	else
	{
		model->setWorldRotation (localRotation);
	}

	Matrix33 worldRotation = model->getWorldRotation ();
	dMatrix3 r;
	r[0]  = worldRotation[0][0];
	r[1]  = worldRotation[0][1];
	r[2]  = worldRotation[0][2];
	r[3]  = 0.0;
	r[4]  = worldRotation[1][0];
	r[5]  = worldRotation[1][1];
	r[6]  = worldRotation[1][2];
	r[7]  = 0.0;
	r[8]  = worldRotation[2][0];
	r[9]  = worldRotation[2][1];
	r[10] = worldRotation[2][2];
	r[11] = 0.0;

	dBodySetRotation (body, r);
}

/*******************************/

void RigidBody::setLocalVelocity (const Vector3& localVelocity)
{
	// assert body

	Vector3 deltaVelocity = localVelocity - this->localVelocity;

	this->localVelocity  = localVelocity;
	this->worldVelocity += deltaVelocity;

	dBodySetLinearVel (body, worldVelocity.x, worldVelocity.y, worldVelocity.z);
}

/*******************************/

void RigidBody::setLocalAngularVelocity (const Vector3& localAngularVelocity)
{
	//assert body

	Vector3 deltaVelocity = localAngularVelocity - this->localAngularVelocity;

	this->localAngularVelocity  = localAngularVelocity;
	this->worldAngularVelocity += deltaVelocity;

	dBodySetAngularVel (body, worldAngularVelocity.x, worldAngularVelocity.y, worldAngularVelocity.z);
}

/*******************************/

void RigidBody::setLocalAcceleration (const Vector3& localAcceleration)
{
}

/*******************************/

void RigidBody::setLocalAngularAcceleration (const Vector3& localAngularAcceleration)
{
}

/*******************************/

void RigidBody::setWorldPosition (const Vector3& worldPosition)
{
	// assert body

	if (island)
	{
		Vector3 deltaPosition = worldPosition - getWorldPosition ();

		for (unsigned int i = 0; i < island->size (); i++)
		{
			RigidBody* rigidBody = island->getRigidBody (i);

			if (rigidBody)
			{	
				dBodyID body = rigidBody->body;

				Spatial* model = rigidBody->model;
				Spatial* parent = model->getParent ();

				Vector3 position = Vector3 (dBodyGetPosition (body)) + deltaPosition;

				dBodySetPosition (body, position.x, position.y, position.z);
				model->setWorldPosition (position);

				if (parent)
				{
					model->setLocalPosition (parent->getWorldRotation ().transpose () * 
							((model->getWorldPosition () - parent->getWorldPosition ()) / parent->getWorldScale ()));
				}
				else
				{
					model->setLocalPosition (model->getWorldPosition ());
				}
			}
		}
	}
	else
	{
		model->setWorldPosition (worldPosition);

		Spatial* parent = model->getParent ();

		if (parent)
		{
			model->setLocalPosition (parent->getWorldRotation ().transpose () * 
					((model->getWorldPosition () - parent->getWorldPosition ()) / parent->getWorldScale ()));
		}
		else
		{
			model->setLocalPosition (model->getWorldPosition ());
		}

		dBodySetPosition (body, worldPosition.x, worldPosition.y, worldPosition.z);
	}
}

/*******************************/

void RigidBody::setWorldRotation (const Matrix33& worldRotation)
{
	// assert body

	Spatial* parent = model->getParent ();

	if (parent)
	{
		model->setLocalRotation (parent->getWorldRotation ().transpose () * worldRotation);
	}
	else
	{
		model->setLocalRotation (worldRotation);
	}

	dMatrix3 r;
	r[0]  = worldRotation[0][0];
	r[1]  = worldRotation[0][1];
	r[2]  = worldRotation[0][2];
	r[3]  = 0.0;
	r[4]  = worldRotation[1][0];
	r[5]  = worldRotation[1][1];
	r[6]  = worldRotation[1][2];
	r[7]  = 0.0;
	r[8]  = worldRotation[2][0];
	r[9]  = worldRotation[2][1];
	r[10] = worldRotation[2][2];
	r[11] = 0.0;

	dBodySetRotation (body, r);

	if (island)
	{
		Matrix33 rotation;
		Vector3  position;

		for (unsigned int i = 0; i < island->size (); i++)
		{
			RigidBody* rigidBody = island->getRigidBody (i);

			if (rigidBody)
			{	
				dBodyID b = rigidBody->body;

				if (b != body)
				{
					Spatial* model = rigidBody->model;
					Spatial* parent = model->getParent ();

					rotation = worldRotation * model->getLocalRotation ();
					position = parent->getWorldPosition () +
						parent->getWorldScale () * (worldRotation * model->getLocalPosition ());

					r[0]  = rotation[0][0];
					r[1]  = rotation[0][1];
					r[2]  = rotation[0][2];
					r[3]  = 0.0;
					r[4]  = rotation[1][0];
					r[5]  = rotation[1][1];
					r[6]  = rotation[1][2];
					r[7]  = 0.0;
					r[8]  = rotation[2][0];
					r[9]  = rotation[2][1];
					r[10] = rotation[2][2];
					r[11] = 0.0;

					dBodySetRotation (b, r);
					model->setWorldRotation (rotation);

					dBodySetPosition (b, position.x, position.y, position.z);
					model->setWorldPosition (position);
				}
			}
		}
	}
}

/*******************************/

void RigidBody::setWorldVelocity (const Vector3& worldVelocity)
{
	// assert body

	Vector3 deltaVelocity = worldVelocity - this->worldVelocity;

	this->localVelocity += deltaVelocity;
	this->worldVelocity  = worldVelocity;

	if (body)
	{
		dBodySetLinearVel (body, worldVelocity.x, worldVelocity.y, worldVelocity.z);
	}
}

/*******************************/

void RigidBody::setWorldAngularVelocity (const Vector3& worldAngularVelocity)
{
	// assert body

	Vector3 deltaVelocity = worldAngularVelocity - this->worldAngularVelocity;

	this->localAngularVelocity += deltaVelocity;
	this->worldAngularVelocity  = worldAngularVelocity;

	dBodySetAngularVel (body, worldAngularVelocity.x, worldAngularVelocity.y, worldAngularVelocity.z);
}

/*******************************/

void RigidBody::setWorldAcceleration (const Vector3& worldAcceleration)
{
}

/*******************************/

void RigidBody::setWorldAngularAcceleration (const Vector3& worldAngularAcceleration)
{
}

/*******************************/

void RigidBody::update (Real deltaTime)
{
	// assert body

	if (0.0 < rollingMu)
	{
		Vector3 av = dBodyGetAngularVel (body);

		if (0.0 < fabs(av.x) || 0.0 < fabs(av.y) || 0.0 < fabs (av.z))
		{
			Vector3 lv = dBodyGetLinearVel (body);
			Vector3 fr = dBodyGetForce     (body);
			Vector3 tr = dBodyGetTorque    (body);

			lv *= -rollingMu;
			av *= -rollingMu;

			dBodyAddForce  (body, lv.x, lv.y, lv.z);
			dBodyAddTorque (body, av.x, av.y, av.z);

			fr = dBodyGetForce     (body);
			tr = dBodyGetTorque    (body);
		}
	}	

	Vector3 worldPosition;
	worldPosition = dBodyGetPosition (body);
	model->setWorldPosition (worldPosition);

	const dReal* r = dBodyGetRotation (body);
	model->setWorldRotation (Matrix33 (
				r[0],  r[1],  r[2], 
				r[4],  r[5],  r[6], 
				r[8],  r[9], r[10]));

	localVelocity        = dBodyGetLinearVel  (body);
	localAngularVelocity = dBodyGetAngularVel (body);

	worldVelocity        = localVelocity;
	worldAngularVelocity = localAngularVelocity;
	
	Spatial* parent = model->getParent ();

	if (parent)
	{
		model->setLocalPosition (parent->getWorldRotation ().transpose () * 
				((model->getWorldPosition () - parent->getWorldPosition ()) / parent->getWorldScale ()));
		model->setLocalRotation (parent->getWorldRotation ().transpose () * model->getWorldRotation ());
	}
	else
	{
		model->setLocalPosition (model->getWorldPosition ());
		model->setLocalRotation (model->getWorldRotation ());
	}
}

/*******************************/
